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摘 要 : [目的 /意义 ] 精准 可 靠 的 定位 技术 是 智能 


导航 系统 (Global Navigation Satellite System, GNSS) 定位 方法 在 农业 环境 中 容易 受到 树木 遮挡 、 电 
响 ， 因 而 ， 提 出 一 种 基于 三 维 激光 雷达 (Light Detection and Ranging, LIDAR) 与 惯性 测量 单元 (Inertial Measure- 
ment Unit, IMU) 信息 融合 的 农业 机 器 人 定位 方法 。[ 方 法 ] 首先 ， 利 用 基于 角度 的 聚 类 方法 对 激光 雷达 点 云 数 据 
进行 信息 处 理 ， 并 与 三 维 正 态 分 布 变换 (3D Normal Distribution Transform, 3D-NDT) 定位 算法 相 结合 ， 在 先 验 点 


lim 
T 
ot 


业 机 器 人 开展 自主 导航 作业 的 重要 前 担 ， 而 常用 的 全 球 卫星 


干扰 等 因素 影 


云 地 图 信息 基础 上 实现 基于 激光 雷达 的 实时 定位 ; 其 次 ， 为 了 克服 单传 感 器 定位 方法 的 局 限 性 ， 利 用 扩展 卡尔 曼 
滤波 (Extended Kalman Filter, EKF) 算法 对 激光 雷达 定位 信息 与 IMU 里 程 计 信息 进行 融合 ， 进 一 步 提 升 农 业 机 器 
人 的 定位 精度 。 最 后 ， 分 别 在 机 器 人 操作 系统 (Robot Operating System, ROS) 的 Gazebo 仿真 环境 中 ， 以 及 真实 作 
业 场 景 中 进行 实验 ， 验 证 提出 的 定位 算法 的 有 效 性 。[ 结 果 和 讨论 ] 融合 定位 方法 在 仿真 环境 中 的 纵向 和 横向 平均 


定位 误差 分 别 为 1.7 和 1.8 cm， 而 在 实验 中 的 纵向 和 横向 平均 定位 误差 分 别 为 3.3 和 3.3 cem， 均 小 于 传统 3D-NDT 定 


位 算法 的 定位 误差 。[ 结 论 ] 提出 的 融合 定位 方法 能 够 满足 农业 机 器 人 在 弱 GNSS 环境 下 


农业 机 器 人 提供 了 一 种 新 的 定位 方法 。 


q 主 作 业 的 定位 要 求 ， 为 
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0 引言 

BEC SAIL AE, BOR RAO. Ri 
等 经 典 农业 场景 开始 逐步 实现 信息 化 与 智能 化 ， 农 
业 机 器 人 代替 人 工 进行 无 人 播种 、 智 能 采摘 、 自 主 
导航 等 任务 。 其 中 ， 农 业 机 器 人 自身 的 精确 定位 是 
实现 这 些 任 务 的 基础 ， 而 传统 的 农业 机 器 人 通常 采 
用 基于 北斗 卫星 导航 系统 (BeiDou Navigation Sat- 
ellite System, BDS) 、 全 球 定 位 系统 (Global Posi- 
tioning System, GPS) 的 全 球 卫 星 导 航 系统 (Glob- 
al Navigation Satellite System, GNSS) 进行 定位 ,但 


收 稿 日 期 : 2024-01-10 


GNSS 的 定位 信号 容易 受到 树木 遮蔽 、 电 磁 干 扰 以 
及 电离 层 的 影响 ， 进 而 导致 定 位 精度 下 降 "”。 为 
了 解决 该 问题 ， 近 年 来 各 国学 者 不 断 提 出 基于 激光 
和 雷达、 摄像 涉 、 惯 性 测量 单元 (Inertial Measure- 
ment Unit, IMU) 、 超 宽带 技术 (Ultra Wide Band, 
UWB) 的 全 新 定位 方法 ， 其 中 ， 激 光 雷 达 定 位 方 
法 因 其 精度 高 、 可 靠 性 好 、 实 时 性 强 等 优点 ， 逐 渐 
在 农业 机 器 人 中 得 到 应 用 和 推广 。 

激光 雷达 定位 方法 是 对 激光 雷达 (Light Detec- 
tion and Ranging, LIDAR) 采集 的 前 后 两 帧 点 云 信 
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息 进 行 对 比 和 匹配 ， 得 到 位 次 变换 关系 ， 进 而 推算 
出 机 器 人 的 当前 位 置 。 目 前 常用 匹配 算法 主要 包括 
迭代 最 近 点 (Iterative Closest Point, ICP) £X iE "^, 
以 及 正 态 分 布 变换 (Normal Distribution Transform, 
NDT) 算法 中 。 为 了 扩大 NDT 算 法 的 使 用 维度 ， 
Saarinen 等 和 Hu 等 "对 NDT 算 法 进行 改进 ， 建 
立 了 3D-NDT 算 法 ,提高 了 算法 的 效率 与 精度 。 男 
外 ， 部 分 学 者 提出 了 基于 先 验 点 云 地 图 的 定位 方 
法 ,将 二 维 激光 雷达 扫描 结果 与 先 验 点 云 地 图 进行 
匹配 ， 从 而 得 到 当前 位 置 “2 。 但 这 些 方法 都 受 限 
于 二 维 激光 雷达 信息 较 少 ， 难 以 处 理 复杂 场景 。 近 
年 来 ， 随 着 机 器 人 计算 性 能 提升 ， 信 息 更 加 丰富 的 
三 维 激光 雷达 也 逐渐 被 应 用 到 农业 机 器 人 的 精确 定 
位 中 。 人 金泰 宇和 黄 劲 松 ”使 用 三 维 激光 雷达 提取 
点 云 中 树木 等 目标 ， 再 与 先 验 点 云 地 图 进行 配 准 ， 
实现 了 更 加 鲁 棒 的 定位 ; 部 分 学 者 则 创新 地 利用 了 
激光 雷达 点 云 的 强度 值 信息 与 先 验 点 云 地 图 进行 匹 
配 ， 在 复杂 场景 下 实现 了 更 加 精确 的 自主 
ret Ue, 

虽然 依靠 激光 雷达 的 定位 算法 已 较为 成 熟 ， 但 
在 实际 使 用 时 仍 会 因为 点 云 畸 变 、 采 样 频率 较 低 等 
问题 影响 定位 精度 。IMU 因 其 采样 频率 高 以 及 位 姿 
解 算 简便 等 原因 受到 众多 学 者 青睐 ， 但 因 其 可 能 存 
在 较 大 的 累积 误差 ， 故 和 常 与 激光 雷达 融合 进行 定 
位 。 崔 文 等 提出 一 种 基于 先 验 点 云 地 图 的 定位 
方法 ， 通 过 误差 卡尔 曼 滤 波 (Error-state Kalman 
Filter, ESKF) 融合 IMU 与 激光 雷达 定位 信息 ， 克 
服 了 无 人 车 在 转 问 时 与 地 图 匹配 误差 较 大 的 问题 。 
aK SC BASE "针对 传统 自 适 应 蒙特 卡 洛 (Adoptive 
Monte Carlo Localization, AMCL) 算法 在 IMU 与 里 
程 计 上 累积 误差 较 大 的 问题 ， 提 出 了 一 种 基于 松 耦 
合 结构 的 扩展 卡尔 曼 滤 波 (Extended Kalman Filter, 
EKF) 和 AMCL 多 传感器 融合 定位 方法 ， 并 结合 激 
光 雷 达 进 行 定 位 ， 提 高 了 定位 精度 。 涂 远 泥 等 UU 
则 通过 联合 无 迹 卡 尔 受 滤波 (Unscented Kalman 
Filter, UKF) 算法 与 粒子 滤波 对 IMU 、 轮 式 里 程 计 
和 激光 雷达 进行 融合 定位 ， 将 定位 误差 稳定 在 了 
2 cm 左右 。 

上 述 人 研究 方法 中 基于 地 图 的 定位 算法 通常 都 对 
激光 雷达 原始 点 云 进行 降 采样 ， 然 后 直接 与 先 验 点 
云 地 图 进行 匹配 求解 位 姿 。 但 在 农业 环境 中 ， 激 光 
雷达 扫描 得 到 的 点 云 数据 一 般 来 自 于 环境 中 比较 明 
显 的 物体 上 ， 如 树木 等 ， 直 接 对 原始 点 云 进 行 降 采 
样 可 能 会 丢失 这 部 分 信息 。 针 对 这 一 问题 ， 本 研究 


对 于 智慧 农业 中 常见 的 果园 环境 进行 点 云 聚 类 ， 保 
留 扫 摘 点 云 中 的 果园 环境 特征 ， 提 高 点 云 与 先 验 点 
云 地 图 匹配 时 的 准确 性 。 然 后 利用 扩展 卡尔 曼 滤 波 
算法 将 3D-NDT 定 位 结果 与 IMU 里 程 计 信息 进行 融 
合 ， 修 正 3D-NDT 定 位 算法 中 的 缺陷 ， 实 现 农业 机 
fit A EAR ER PNE P BUTS E DI. 


1 材料 与 方法 
1.1 仿真 与 实验 平台 


本 研究 的 仿真 实验 主要 通过 机 髓 人 操作 系统 
(Robot Operating System, ROS) 平台 上 的 Gazebo 软 
件 搭 建 农业 机 器 人 模型 及 农业 作业 环境 开展 。 其 
中 ,农业 机 器 人 模型 的 底盘 采用 差 速 移动 模型 ， 搭 
载 VLP-16 激 光 雷 达 与 IMU 用 于 仿真 定位 实验 ; 在 
农业 仿真 场景 中 设置 了 丘陵 地 形 ED EIC 
平房 模拟 真实 环境 ， 同 时 设置 了 较 多 树木 用 于 模拟 
某 些 环境 较为 复杂 的 农业 场景 ， 如 图 1 所 示 。 仿真 
实验 平台 采用 搭载 AMD Ryzen7 4800 H 处 理 器 、 
16 G 运 行内 存 、NVIDIA GeForce RTX 2060 显卡 ， 
操作 系统 为 Ubuntul18.04 的 计算 机 。 


图 1 农业 环境 的 仿真 模型 


Fig. 1 Simulation model of the agricultural environment 


为 进一步 验证 本 研究 所 提 融 合 定位 算法 的 实际 
可 行 性 ， 以 自主 研发 的 农业 机 器 人 (智能 割 草 机 ) 
作为 测试 平台 进行 实验 ， 其 整体 结构 如 图 2 所 示 。 
测试 平台 为 后 轮 差 速 驱 动 ， 通 过 遥控 设备 发 送 脉冲 
宽度 调制 (Pulse Width Modulation, PWM) 信号 ， 
在 Arduino PAW IP, Fillet RS232 $ O £z 3$ 5 
JETSON XAVIER NX Alb Zi 4s, FRI iE EB ET BM [8] 
Keya XX 3K fri] HI C2] it Ac 38 t th] $8 T A re e d n 
驱动 电机 实现 直行 、 转 向 等 操作 。 实 验 所 用 激光 雷 
达 为 Velodyne 公司 生产 的 16 线 激光 雷达 VLP-16; 
IMU 为 VectorNav Technologies 公司 生产 的 VN-200 
小 型 惯性 导航 系统 ; 为 验证 定位 结果 准确 性 ， 以 
Witmotion 公 司 生 产 的 实时 动态 载波 相位 差分 (Re- 
al-Time Kinematic, RTK) 定位 系统 输出 的 定位 结 
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作为 基准 ， 并 将 其 与 另外 两 种 定位 结果 进行 比较 
分 析 。 


图 2 智能 割 草 机 器 人 测试 平台 
Fig. 2 Test platform of intelligent mowing robot 


1.2 激光 雷达 点 云 聚 类 


1.2.1 点 云 数据 预 处 理 

三 维 激光 雷达 扫描 的 原始 点 云 数 据 量 较 大 ， 直 
接 对 其 进行 数据 处 理会 极 大 增加 计算 时 间 ， 从 而 影 
响 农 业 机 器 人 的 作业 实时 性 。 本 研究 采用 深度 图 方 
法 对 三 维 点 云 数 据 进 行 保存 ， 根 据 激光 雷达 竖 直 
和 水 平分 辩 率 得 到 二 维 图 像 分 辩 率 大 小 ， 通 过 公 
X (1) 将 激光 雷达 三 维 点 云 数据 转换 为 点 云 到 激 
光 雷 达 的 距离 数据 ， 并 依次 填 入 二 维 图 像 中 得 到 深 
度 图 。 其 中 ， 深 度 图 的 每 一 行 和 每 一 列 分 别 代表 水 
平和 竖 直 方向 上 扫描 得 到 的 点 云 距 离 数据 。 通 过 该 
方法 不 仅 可 以 将 点 云 的 三 维 数据 转换 为 二 维 数据 ， 
还 能 将 原本 无 序 排列 的 点 云 有 序 化 为 二 维 数 组 的 形 
式 ， 简 化 了 后 续 聚 类 点 云 的 搜索 方式 ， 缩 短 聚 类 
时 间 。 

range = jx! +y +z’ (1) 

式 中 : x、y、z 表 示 激 光束 打 在 环境 物体 上 得 
到 的 激光 点 空间 坐标 值 ; range (i, j) 表示 深度 图 
中 第 i 行 ， 第 j 列 的 点 到 激光 雷达 的 距离 ;i 表示 深 
度 图 形 分 辩 率 也 即 激光 雷达 的 线束 值 ， 以 16 线 激 
光 雷 达 为 例 ， 此 时 i=16; 7 代表 深度 图 列 分 辩 率 ， 
与 激光 雷达 水 平分 辨 率 有 关 ，16 线 激光 雷达 中 水 平 
分 辨 率 为 0.2"， 此 时 六 (360°+0.2°) =1 800。 深 度 
图 的 示意 图 如 图 3 所 示 。 同 理 ， 也 可 以 根据 此 变换 
关系 从 深度 图 中 根据 行列 关系 检索 到 原 有 点 云 数 
据 ， 使 深度 图 距离 数据 与 点 云 三 维 数据 建立 联系 。 
12.2 ”地 面 点 云 去 除 

在 三 维 激 光 雷 达 原 始点 云 中 ,地面 点 云 数据 占 
比较 大 ， 会 增 大 数据 处 理 运 算 量 。 另 外 ， 在 聚 类 过 
程 中 不 需要 处 理 地 面 点 云 信 息 。 因 此， 在 进行 点 云 
聚 类 前 需要 先 去 除 地 面 点 云 。 由 深度 图 原理 可 知 ， 


图 3 激光 雷达 点 云 的 深度 图 
Fig. 3 Depth map of LiDAR point cloud 


每 一 列 数据 代表 竖 直 方向 上 扫描 得 到 的 点 云 值 。 假 
设 激光 雷达 在 竖 直 方向 上 扫描 得 到 的 相 邻 两 点 分 别 
HA. BAAR, EEDI H A(x, Yo Zo) 
B(x, Yo Zz) WAS ra. 


d 一 ga 
/ \ 
En poses 
V. Axa T 
\ \ 2 if / / 
x bs NO f 2 T 
N Ne Nes pes 
Rie. € eg Ead 


图 4 激光 雷达 地 面 线束 示意 图 
Fig. 4 Ground scanning beams of LiDAR 


则 A、B 两 点 竖 直 方向 的 高 度 差 h.、 水 平 距 离 


差 dy,、 两 点 连 线 与 水 平面 的 夹 角 0 可 以 分 别 表示 为 
公式 (2) 一 公式 OD. 


h.=|zo— zi] (2) 
dip = J (4-11 + Oo -yy) (3) 
0 = arctan (h,/d 4, ) (4) 


式 中 : 两 点 连 线 与 水 平面 的 夹 角 9 可 以 用 来 评 
佑 作业 地 面 的 平坦 程度 ， 如 果 激 光 雷 达 水 平安 装 且 
地 面 平坦 ， 夹 角 0 理论 值 应 该 为 0"。 考 虑 实际 情 
JL, 设置 地 面 角度 阔 值 为 10*， 当 |9| 小 于 该 阔 值 
时 ,认为 4、B 两 点 属于 地 面 点 。 通 过 裔 历 深度 图 ， 
可 以 得 到 分 割 出 的 地 面 点 云 结果 (RS). Hop, X 
色 部 分 代表 地 面 的 点 云 信 息 。 
1.2.3 m AREE 

农业 机 器 人 上 的 三 维 激光 雷达 点 云 聚 类 采用 
Bogoslavskyi 和 Stachniss 2” 提出 的 基于 角度 的 聚 类 
方法 。 该 方法 对 于 点 云 间 的 角度 关系 较为 敏感 ， 特 
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图 5 激光 雷达 的 地 面 点 云 分 割 结果 
Fig. 5 Segmentation result of LIDAR ground point cloud 
别 适用 于 检测 近似 垂直 于 地 面 的 物体 。 在 农业 环境 
下 ， 大 多 数 作 物 的 树干 都 具有 这 类 特征 ， 能 人 够 在 聚 
类 过 程 中 被 准确 识别 ， 从 而 在 一 定 程 度 上 还 原 环境 
的 真实 性 。 该 聚 类 算法 原理 如 图 6 所 示 。 


注 : A 点 和 B 点 为 深度 图 中 任意 两 个 相 邻 点 ,OO 点 为 激光 雷达 坐标 原 
点 ,连接 OA、0B 与 4B 形 成 一 个 三 角形 , 设 OA 为 较 长 边 ,过 B 点 作 
OA 垂 线 , 重 足 为 C 点 ,PB 为 O04 与 BA 来 角 ,a 为 激光 雷达 水 平分 辨 率 
或 重 直 分 状 率 对 应 角度 。 

图 6 角度 聚 类 原理 图 


Fig. 6 Schematic diagram of angular clustering 


图 6 中 得 出 8 的 表达 式 如 公式 (5) 所 示 。 


| OB lsina 
|| OA || - | OB [cosa 

式 中 : 04 与 08B 的 大 小 即 为 深度 图 中 对 应 的 距 
离 值 ， 可 以 通过 查找 深度 图 直接 获得 。 设 置 p 角 的 
一 个 阔 值 Yy， 当 B>y 时 ， 认 为 4 点 与 巨 点 相对 平坦 ， 
将 其 归 为 同一 类 别 ; 当 pB<xy 时 ,认为 A 点 与 B 点 之 
间 的 距离 相差 过 大 ， 将 两 者 视 为 不 同类 别 。 

根据 深度 图 的 特点 ， 采 用 基于 图 的 广度 优先 搜 
索 (Breadth First Search, BFS), JA JE — JT ox 
度 图 中 上 下 左右 4 个 领域 点 进行 遍历 ， 在 遍历 完 所 
有 点 云 后 得 到 最 终 的 聚 类 结果 ， 如 图 7 所 示 。 通 过 
基于 角度 的 点 云 聚 类 ， 可 以 将 农业 环境 中 许多 明显 


(5) 


arctan ( 


的 特征 保留 下 来 ， 如 环境 中 的 树 、 边 界 等 。 传 统 的 
3D-NDT 点 云 匹 配 算法 对 激光 雷达 采集 的 点 云 数 据 
进行 简单 降 采 样 后 ， 直 接 与 先 验 点 云 地 图 匹配 。 而 
本 研究 方法 将 采集 的 点 云 数据 先 转换 为 肾 类 后 的 点 
云 ， 再 与 点 云 地 图 进行 匹配 ， 可 以 充分 利用 更 多 的 
环境 特征 ， 提 高 定位 系统 的 准确 性 。 


图 7 激光 雷达 点 云 的 聚 类 结果 
Fig. 7 Clustering result of LiDAR point cloud 


1.2.4 ”改进 的 3D-NDT 定位 算法 

3D-NDT 匹 配 算法 是 一 种 通过 概率 密度 函数 将 
两 帧 点 云 进行 匹配 并 求解 变换 位 姿 的 点 云 匹 配 算 
法 。 该 算法 使 用 统计 模型 并 通过 最 优化 算法 确定 配 
准 的 最 优 转 换 参数 ， 充 分 利用 了 点 云 的 概率 分 布 ， 
鲁 棒 性 好 且 实 时 性 强 3 光 。 针 对 农业 作业 场景 的 特 
殊 性 ， 本 研究 利用 第 1 一 3 节 提 出 的 聚 类 方法 ， 对 
激光 雷达 扫描 得 到 的 原始 点 云 进 行 聚 类 ， 保 留 环境 
中 树木 、 浇 木 从 等 明显 特征 ， 还 原 部 分 农业 场景 ， 
再 作为 输入 点 云 与 先 验 点 云 地 图 进行 匹配 ， 提 高 算 
法 的 准确 性 。 

基于 先 验 点 云 地 图 的 3D-NDT 定 位 方法 是 3D- 
NDT 匹 配 算法 在 定位 技术 上 的 应 用 ， 该 定位 方法 将 
激光 雷达 收集 到 的 每 一 帧 点 云 与 提前 准备 好 的 先 验 
点 云 地 图 进行 匹配 计算 求解 位 姿 ， 实 现 自身 定位 。 
具体 流程 如 下 : 将 先 验 点 云 地 图 中 的 点 云集 合 划 分 
为 多 个 网 格 ， 其 中 ， 某 个 网 格 中 的 点 云集 合 为 
X xl, xx], x ORAE GU PURA n I HE 
坐标 。 假 设 该 点 云集 合 符合 高 斯 分 布 ， 则 通过 公 
X (6) 和 公式 CD 计算 三 维 网 格 中 的 点 云 对 应 的 
方差 wp 与 均值 2， 并 通过 公式 (8) 构建 概率 密度 函 
BU (x). 


p= Lx, (6) 
pel 


Se ow (7) 
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PT SS wd EM 
Qz)? A5] 
(8) 
不 同 于 原始 点 云 数 据 ， 使 用 概率 密度 函数 广 (x) 
表示 网 格 中 的 点 云 信 息 时 ， 通 常 将 参考 点 云 近似 为 
一 个 局 部 表面 ， 并 反映 该 表面 的 平滑 程度 与 方向 信 
息 。 这 样 不 仅 增加 了 先 验 点 云 地 图 的 信息 丰富 度 ， 
同时 ， 使 用 概率 密度 还 可 降低 三 维 数据 的 运算 
成 本 。 
3D-NDT 匹 配 算法 的 目标 是 通过 调整 位 姿 变换 
参数 ， 使 得 位 姿 变换 后 的 点 云 在 网 格 概率 密度 函 
数 中 计算 的 概率 最 大 ， 并 输出 此 时 的 最 佳 位 姿 变 
换 。 在 农业 机 器 人 的 运动 中 ， 通 常 采 用 平移 量 以 
及 欧 拉 角 表示 其 位 姿 变换 ， 主 要 包含 X、Y、Z 坐 
标 轴 上 的 3 个 平移 参数 与 3 个 旋转 参数 。 该 变换 关 
AW WO fu Xe 3B p HEAT Rm, p= 
[to f. to Po Pp Plo SBOE IK zs t I BE 
进行 预 处 理 和 聚 类 ， 得 到 3D-NDT 匹 配 算法 的 输入 
ROB Ylyxlye Yo co y HE MA om 


Y(y.]ye Yoo ccs Yn} 按照 位 姿 变 换 p 得 到 点 去 
Yyy ys … x) EHE SEU rh, nA 
X (9) 一 公式 (12) 所 示 。 
y/-2 T(p.y)2t,,.* R.R,R,y, (9) 
1 0 0 
R,=|9 cosg, A (10) 
[0 sing, coso, 
| cosp, 0 sing, 
R =| 0 1 0 (11) 
|-sing, 0 bi 
| cos 9. -sin 9, | 
R.-|sing, cosg, 0 (12) 


0 0 1 

式 中 : t., .表示 y, 的 平移 变换 ;，R,，R,，R. 则 
表示 y, 的 旋转 变换 。 得 到 点 云 变 换 后 的 yy， 通 过 概 
率 密度 函数 (x) 计算 出 变换 后 的 点 云集 合 
YU yy. yoy} 在 该 网 格 下 的 概率 S(p)， 如 公 


式 (13) 所 示 。 
S(p) =] | FT y) = 


wD Oi - a) 
2 


[ ] 


m 1 
[exp 
FA (2n)*,/(>)| 


(13) 


为 了 简化 后 续 计算 步 又 ， 
对 数 得 到 公式 (14)。 


in5(p)- SOL > E eam 


由 于 公式 (04) 中 C 为 常数 项 ， 为 使 概率 
S$(p) 达 到 最 大 ， 设 定 优化 目标 如 公式 (15) 所 示 


min Sr -aV Y, Gt - a) (15) 


NP: y! 只 与 变换 位 盗 p HK, a E XS iit 
修正 变换 位 盗 p， 获 得 概率 5S (p) BCK a^ SE IX 
数 时 的 匹配 结果 ， 即 可 得 到 当前 最 佳 位 姿 变 换 ， 具 
体 算法 流程 如 图 8 所 示 。 通 过 不 断 获 取 激 光 雷 达 输 
入 的 原始 点 云 数据 并 得 到 聚 类 点 云 后 ，3D-NDT 匹 
配 算法 可 将 聚 类 点 云 作为 输入 点 云 与 先 验 点 云 地 图 
进行 匹配 并 计算 出 当前 位 姿 变换 情况 ， 通 过 对 位 姿 
变换 进行 累积 即 可 实现 农业 机 器 人 的 精确 定位 。 


激光 雷达 


对 公式 (03) 两 边 取 


网 格 划 分 


构建 概率 密度 函数 flx) 聚 类 点 云 


; < 


Y 
求解 位 姿 变换 p 


Y 
输出 最 佳 位 姿 变 化 p 


图 8 改进 3D-NDT 算 法 流程 图 
Fig. 8 Flowchart of the improved 3D-NDT algorithm 


由 图 8 可 知 ， 采 用 3D-NDT 定 位 算法 进行 定位 
时 ， 比 较 依 赖 于 机 顺 人 的 初始 位 姿 ， 当 初始 位 姿 与 
地 图 难以 匹配 时 ,会 造成 较 大 累积 误差 。 部 分 研究 
采用 GPS Be IMU 信息 作为 初始 位 姿 以 减 小 累积 误 
差 。 除 此 之 外 ， 对 于 一 些 复杂 农业 作业 环境 ， 在 先 
验 点 云 地 图 划分 过 程 中 ， 网 格 分 辩 率 大 小 的 设置 往 
往 难以 兼顾 全 局 ， 也 会 存在 部 分 场景 网 格 分 辨 率 过 
小 或 过 大 的 问题 ， 从 而 影响 点 云 匹配 精度 并 产生 累 
只 误差 。 针 对 这 一 问题 ， 本 研究 使 用 扩展 卡尔 曼 滤 
波 方 法 ， 在 改进 后 3D-NDT 定 位 算法 的 定位 结果 基 
础 上 ， 进 一 步 融 合 IMU 里 程 计 信息 , 减 小 了 累积 误 
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差 产生 的 影响 。 
1.3 基于 EKF 的 激光 雷达 与 IMU 融合 定位 


基于 滤波 的 融合 定位 算法 的 核心 思想 是 通过 对 
系统 的 运动 进行 预测 估计 ， 再 集合 其 他 传感器 所 输 
出 的 观测 值 进行 更 新 修正 ， 实 现 对 系统 运动 状态 的 
最 优 估 计 ， 完 成 融合 定位 。 其 中 ，EKF 使 用 泰勒 展 
开 将 非 线 性 系统 线性 化 后 再 进行 状态 估计 ， 适用 于 
具有 非 线 性 动力 学 特性 的 农业 机 器 人 7077. TTI 
将 系统 预测 模型 和 观测 模型 等 先 验 信息 与 真实 观测 
数据 相 结 合 ， 获 得 系统 状态 的 最 优 估计 并 不 断 更 新 
状态 估计 和 协 方差 和 矩阵， 从 而 改进 对 状态 的 估计 ， 
并 提高 算法 的 准确 性 和 稳定 性 。 

1.3.1 基于 EKF 的 最 优 估 计 流 程 

对 非 线性 模型 进行 线性 化 处 理 之 后 ，EKF 算 法 
的 流程 主要 包括 系统 预测 方程 以 及 观测 方程 初始 
化 、 预 测 以 及 更 新 3 个 步 又 。 有 具体 内 容 如 下 : 

1) 初始 化 。 基 于 系统 及 传感器 的 实际 工作 情 
况 ， 构 建 预测 方程 f(-) 以 及 观测 方程 4(:)， 结 合 系 
统 状态 量 、 观 测量 ， 通 过 预测 与 更 新 对 估计 结果 进 
行 修正 。 

2) 预测 。 基 于 上 一 时 刻 的 最 优 估计 值 *,.，， 
利用 预测 方程 计算 当前 时 刻 的 先 验 估计 值 站 ， 对 当 
前 状态 量 进行 初步 估计 ， 并 结合 上 一 时 刻 的 协 方差 
EEP, ,推算 当前 时 刻 的 先 验 协 方差 矩阵 Pi， 为 后 
续 更 新 步 提供 计算 基础 。 

3) 更 新 。 通 过 观测 方程 得 到 传感器 对 当前 时 
刻 的 观测 值 z,， 并 在 将 非 线 性 系统 线性 化 后 ， 结 合 
预测 的 先 验 协 方 差 矩 阵 Pi 得 到 观测 方程 的 卡尔 曼 
增益 K.， 进 而 得 到 当前 时 刻 对 应 的 最 优 估计 值 语 ， 
完成 基于 EKF 的 数据 融合 。 同 时 ， 也 可 以 得 到 当前 
时 刻 的 协 方差 矩阵 P,， 以 便 实现 对 系统 下 一 时 刻 的 
状态 估计 。 

在 上 述 步 又 中 ，EKF 算 法 充分 利用 了 系统 上 一 
时 刻 的 状态 量 及 当前 时 刻 的 观测 量 ， 实 现 对 农业 机 
器 人 的 精确 定位 。 

1.3.2 ”基于 EKF 的 传感器 融合 定位 

通过 激光 雷达 与 先 验 点 云 地 网 的 3D-NDT 定 
位 ， 能 够 实现 农业 机 器 人 的 初步 定位 。 在 此 基础 
上 ， 再 使 用 EKF 算 法 进一步 融合 IMU 的 里 程 计 信 
息 ， 从 而 弥补 激光 雷达 定位 中 可 能 产生 的 定位 误 
差 ， 使 农业 移动 机 右 人 获得 更 高 的 定位 精度 。 考 虑 
激光 雷达 与 IMU 的 采样 频率 差异 ， 本 研究 采用 
IMU 的 状态 估计 作为 农业 机 器 人 系统 的 预测 部 分 , 


观测 部 分 则 使 用 激光 雷达 基于 3D-NDT 的 定位 结 
来 更 新 预测 值 ， 融 合 两 者 信息 得 到 最 优 位 姿 。 根 据 
农业 机 器 人 的 动力 学 模型 及 传感器 特性 ， 基 于 
EKF 建立 对 应 预测 方程 与 观测 方程 如 公式 (16) 
和 公式 (17) 所 示 。 

X f(x, pU) + w, (16) 

Z,=h(x,) + v, (17) 

AP: x, Ak b Zl AY 4 ME BL aR A M s 
x, Jk - 工时 刻 的 农业 机 器 人 位 置 的 最 优 估计; u, 
为 已 知 的 系统 输入 量 ; f C) OU AER TE AK Be THM BR 
数 ; wm, 为 系统 状态 噪声 ; h(-) 为 非 线性 系统 观测 函 
JU; 有 为 激光 雷达 在 上 时 刻 的 定位 信息 ; v AA 
统 观测 噪声 。 根 据 农业 机 器 人 的 实际 使 用 情况 ， 
假设 w 与 v 都 满足 均值 为 0 的 高 斯 分 布 ， 则 有 公 
X (18) 和 公式 (19): 

w,~N (0, Q,) (18) 

vi~N (0, R,) (19) 

XP: 9, GR, HTM KBAR AS URS AY BP ZT 
25 B PE- DU t D E P] 7; 2: 4B Vk, MY BAR TE A 
统 ， 对 预测 方程 与 观测 方程 使 用 一 阶 泰勒 展开 ， 忽 
略 高 阶 项 后 得 到 线性 预测 方程 与 线性 观测 方程 ， 如 
公式 (20) 和 公式 (21) 所 示 。 

~f, su) VÉ (x, o£ tw, (20) 

zı =~ h(X;) + VA, (xp Ri) + v, (21) 

式 中 : 总 表示 k 时 刻 位 置 的 先 验 佑 计 值 ; ea 
表示 k -1 时 刻 最 优 估 计 ; YA 表示 预测 函数 的 雅 可 
HIERE; YA 表示 观测 方程 的 雅 可 比 和 矩阵 。 在 构建 
完成 预测 方程 与 观测 方程 并 将 其 线性 化 后 ， 可 以 通 
过 预测 步 与 更 新 步 来 估计 最 优 状 态 。EKF 算 法 预测 
步 与 更 新 步 如 公式 (22) 一 公式 (26) 所 示 。 


u 


X, =f iX.) (22) 
P; = Vf.P,_: Vf." + Q, (23) 
K, = P;Vh'(Vh,P;Vh' + R,y! (24) 
¥,= 4, + K,(z, - h(£;)) (25) 
P, - (I - K,Vh,)P; (26) 


式 中 : PIRR kA A MT 2: 8B E AY Do d fh 
fH; P, Nk - 工时 刻 对 应 的 协 方差 矩阵 ; KA kH 
刻 的 卡尔 曼 增益 ; ,为 EKF 滤 波 后 得 到 的 k 时 刻 最 
优 状 态 估计 值 ， 即 农业 机 器 人 当前 位 置 的 最 优 佑 计 
值 。 公 式 (22) 和 公式 (23) 表示 EKF 算 法 中 的 预 
测 步 ， 主 要 根据 系统 预测 方程 以 及 上 一 时 刻 的 最 
优 佑 计 得 到 当前 时 刻 的 先 验 估计 ; 公式 (24) ~ 
公式 (26) 表示 EKF 算 法 中 的 更 新 步 ， 通 过 观测 方 


100 智慧 农业 (中 英文 ) Smart Agriculture 


程 以 及 各 先 验 估计 值 完 成 对 当前 时 刻 各 状态 量 的 更 
新 修正 ， 在 构建 完成 系统 的 观测 方程 与 预测 方程 
后 ， 通 过 IMU 里 程 计 信息 的 预测 以 及 激光 雷达 3D- 
NDT 定 位 的 更 新 ， 可 以 不 断 将 传感器 定位 信息 进行 
融合 ， 得 到 更 高 精度 的 定位 结果 。 基 于 EKF 算 法 的 
传感器 融合 定位 的 整体 流程 如 图 9 所 示 。 


EKF 融 合 定位 结果 


图 9 EKF 融合 定位 流程 图 
Fig. 9 Flowchart of the EKF location algorithm 


2 结果 与 分 析 


2.1 仿真 结果 与 分 析 

对 本 研究 提出 的 融合 定位 算法 进行 仿真 分 析 ， 
在 1.1 节 中 已 构建 的 农业 仿真 环境 下 ， 选 择 常 见 的 
转向 路 径 和 和 形 路 径 作 为 轨迹 ， 探 制 农业 机 器 人 以 
线 速度 0.8 m/s 按 两 种 轨迹 行驶 ， 分 别 通过 传统 3D- 
NDT 定 位 方法 与 本 研究 提出 的 融合 定位 算法 获取 农 
业 机 器 人 的 位 置信 息 并 与 Gazebo 自 带 的 定位 结 
进行 轨迹 对 比分 析 。 结 果 如 图 10 所 示 ， 其 中 ， 红 
色 曲 线 代 表 Gazebo 提供 的 定位 信息 ， 视 为 农业 机 
器 人 的 真实 位 置 ， 蓝 色 曲 线 为 3D-NDT 算 法 定位 结 
果 ; 绿色 曲线 为 本 研究 提出 的 EKF 融 合算 法 定位 
结果 。 

由 图 10 左 侧 整 体 图 可 知 ， 在 整个 运动 过 程 中 ， 
两 种 定位 方法 得 到 的 定位 结果 与 真实 值 之 间 总 体 相 
差 不 大 ， 轨 迹 基本 一 致 。 而 由 图 10 右 侧 局 部 图 可 
知 ， 当 农业 机 器 人 做 直线 运动 时 ， 两 种 定位 方法 的 
定位 结果 基本 与 真实 值 吻合 ， 误 差 较 小 。 但 是 ， 当 
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a. 转 向 路 径 


b. EBLE 
图 10 仿真 环境 下 农业 机 器 人 的 定位 轨迹 
Fig. 10 Localization trajectory of agricultural robot in Simulation 


出 现 转向 行为 时 ， 采 用 3D-NDT 算 法 得 到 的 定位 结 
R ORE) 明显 偏离 真实 定位 值 (红色 )， 而 EKF 
融合 定位 结果 (绿色) 与 真实 定位 结果 (红色 ) 3E 
本 重合 。 可 以 看 出 ， 利 用 EKF 算 法 融合 两 种 定位 信 
息 后 ， 对 3D-NDT 定位 算法 的 一 些 缺 陷 进 行 了 弥 
补 ， 提 高 了 定位 精度 。 对 两 种 算法 的 定位 结果 进行 
定量 计算 和 比较 ， 得 到 如 图 11 所 示 的 定位 轨迹 。 
由 图 11 中 各 定位 点 分 布 情况 可 知 ， 宏 观 上 ， 绿 
色 轨 迹 位 于 红色 轨迹 与 蓝 色 轨迹 之 间 ， 表 示 EKF 融 
合 定 位 算法 所 得 的 位 置 Pa; 相 比 于 传统 3D-NDT 算 
法 得 到 的 位 置 Py:， 更 接近 真实 值 P,,.， 其 主要 原 
因 在 于 发 生 转 向 行为 时 ，3D-NDT 算 法 会 存在 较 大 
的 定位 误差 ;而 微观 上 ， 对 定位 误差 进一步 分 析 ， 
按照 公式 (27) 和 公式 (28) 计算 Pr 以 及 Pie 与 
真实 值 Pi,. 之 间 的 误差 绝对 值 。 
error_NDT = | P, - Po | (27) 
error_EKF =| Py - Poel (28) 
考虑 农业 机 器 人 在 实际 定位 中 主要 依靠 X 和 了 
方向 上 的 精度 ， 因 此 主要 对 X 和 了 Y 方 向 上 的 误差 绝 
对 值 进 行 分 析 ， 结 果 如 图 12 所 示 。 其 中 ， 红 色 和 
紫色 分 别 表 示 传 统 3D-NDT 定 位 结果 在 X 和 了 方向 
上 的 误差 绝对 值 ;， 而 绿色 和 蓝 色 则 分 别 表 示 EKEF 融 
合 定位 结果 在 X 和 了 方向 上 的 误差 绝对 值 。 具 体 误 
差 绝 对 值 见 表 1。 
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注 : 农业 机 器 人 在 仿真 环境 中 的 真实 位 置 P，, 传 统 3D-NDT 算 法 得 到 的 位 置 Pbr, 以 及 EKF 融 合 定位 算法 得 到 的 位 置 Pexs, 分 别 用 红色 、 蓝 
色 和 绿色 点 表示 。 
图 11 仿真 环境 下 农业 机 器 人 的 定位 结果 


Fig. 11 Localization result of agricultural robot in simulation environment 
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图 12 仿真 环境 下 农业 机 器 人 的 定位 误差 
Fig. 12 Localization error of the agricultural robot in simulation environment 
R1 仿真 环境 下 农业 机 器 人 的 定位 误差 统计 表 
Table 1 Localization error statistical table of agricultural robot in simulation environment 
m fk X 方 向 上 的 平均 误差 了 方向 上 的 平均 误差 X 方 向 上 的 最 大 误差 了 方向 上 的 最 大 误差 
ii 绝对 值 /em 绝对 值 /em 绝对 值 /cm 绝对 值 /em 
3D-NDT 定 位 算法 5.8 3.7 11.7 9.9 
转向 路 径 ey 
EKF 融 合 定位 算法 1.7 2.4 4.9 5.6 
. 3D-NDT 定 位 算法 6.2 3.2 15.3 10.0 
JEJE SAE sa RUS pa 
EKF 融 合 定位 算法 1.7 1.1 4.6 4.7 
3D-NDT 定 位 算法 6.0 3.5 15.3 10.0 
综合 两 种 工 况 Media 
EKF 融 合 定位 算法 1.7 1.8 4.9 5.6 


算法 性 能 提升 /% 71.7 48.6 
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从 图 12 可 以 看 出 ， 在 整个 运动 过 程 中 ， 通 过 
3D-NDT 定位 算法 得 到 的 位 置 误差 绝对 值 波动 较 
大 ， 且 较 明 显 的 数值 波动 也 主要 出 现在 转向 处 。 这 
一 现象 与 激光 雷达 扫描 方式 以 及 3D-NDT 定 位 算法 
本 身 有 关 。 在 对 输入 点 云 的 当前 帧 与 上 一 帧 进行 匹 
配 时 ， 明 显 的 转向 行为 可 能 会 导致 角度 变化 过 大 而 
发 生 畸 变 ， 使 得 点 云 匹 配 准 确 度 下 降 ， 无 法 及 时 输 
出 准确 位 姿 并 产生 累积 误差 。 男 外 ， 在 激光 雷达 的 
采样 频率 较 低 的 情况 下 ， 也 会 导致 3D-NDT 定 位 算 
法 的 误差 绝对 值 曲线 出 现 较 大 波动 。 通 过 EKF 算 法 
将 3D-NDT 定 位 算法 得 到 的 定位 信息 与 IMU 的 里 程 
计 信 息 进行 融合 ， 通 过 对 位 姿 进行 修正 可 以 减 小 该 
累积 误差 ， 减 小 误差 绝对 值 曲 线 的 波动 ， 提 高 整个 
系统 的 定位 准确 性 。 

综合 两 种 工 况 ， 传 统 3D-NDT 定 位 算法 在 和 
7 方向 上 的 平均 定位 误差 分 别 为 60 和 3.5 cm; X fll 
了 方向 上 的 最 大 误差 绝对 值 分 别 为 15.3 和 10.0 cm. 
在 此 基础 上 ， 本 研究 提出 的 算法 通过 提前 进行 点 云 
聚 类 提高 了 3D-NDT 算 法 的 匹配 准确 度 ， 并 使 用 
EKF 算 法 融合 IMU 里 程 计 信息 对 位 姿 进 行 最 优 估 
计 ， 在 X 和 了 方向 上 的 平均 定位 误差 分 别 为 1.7 和 
1.8 cm; 和 X 和 了 方向 上 的 最 大 误差 绝对 值 分 别 为 4.9 
和 5.6 cm。 满 足 农业 机 器 人 作业 需求 的 同时 ， 提 高 
了 作业 时 的 定位 精度 。 

2.2 实验 结果 与 分 析 

为 验证 提出 的 融合 定位 算法 的 实际 工作 性 能 ， 
使 用 自主 研发 的 农业 机 器 人 在 如 图 13 所 示 的 环境 
中 进行 实验 。 

首先 通过 遥控 器 控制 农业 机 需 人 在 该 区 域 运 
动 ， 并 利用 激光 雷达 扫描 和 采集 所 有 环境 点 云 数 
据 ， 然 后 通过 SC-Lego-LOAM 算 法 进行 建 图 ， 生 成 


a. 转 向 路 径 


图 13 农业 机 器 人 定位 实验 环境 
Fig. 13 The environment of agricultural robot localization 


experiment 


先 验 点 云 地 图 ， 如 图 14 所 示 。 


图 14 农业 机 器 人 定位 实验 环境 的 先 验 点 云 地 图 
Fig. 14 Prior point cloud map for environment of agricultural 


robot localization experiment 


参考 仿真 软件 中 设置 的 转向 路 径 与 矩形 路 径 , 
通过 遥控 需 手 动 控 制 农业 机 器 人 以 线 速度 0.8 m/s 分 
别 沿 上 述 两 种 路 径 运 动 ， 输 出 的 定位 结果 如 图 15 
所 示 ， 其 中 红色 曲线 为 RTK 获得 的 真实 定位 轨迹 ; 
蓝 色 曲线 为 3D-NDT 算 法 定位 轨迹 ; 绿色 曲线 为 本 
研究 提出 的 EKF 融 合算 法 定位 轨迹 。 


b. 4E W BIE 


图 15 实验 环境 下 农业 机 器 人 的 定位 轨迹 


Fig. 15 Localization trajectory of agricultural robot in experimental environment 
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由 图 15 可知 ， 实 验 结果 与 仿真 结果 类 似 ， 相 果 仍 然 会 因为 激光 雷达 扫描 方式 存在 一 定 误差 。 对 


比 于 3D-NDT 定 位 结果 ，EKF 融合 定位 结果 更 接近 
真实 值 ， 且 在 农业 机 右 人 转向 时 ，3D-NDT 定 位 结 
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a. 转 向 路 径 


3D-NDT 定 位 算法 与 EKF 融 合 定 位 算法 获得 的 两 种 


定位 结果 进行 


定量 分 析 ， 如 图 16 所 示 。 


8} 真实 坐标 
定位 于 上 


N 


b. 4E BE 


图 16 实验 环境 下 农业 机 器 人 的 定位 结果 
Fig. 16 Localization result of the agricultural robot in experimental environment 


由 图 16 可 知 ， 相 比 于 仿真 环境 ， 在 真实 环境 
中 3D-NDT 定 位 结 会 得 到 一 定 程度 的 改善 ， 其 主 
要 原因 是 真实 环境 中 的 各 种 线 、 面 特征 会 更 加 明显 
与 密集 ， 可 以 提高 点 云 死 配 时 的 匹配 精度 。 但 3D- 
NDT 定 位 方法 在 转向 处 仍然 存在 一 定 偏差 ， 而 在 融 
合 了 IMU 的 里 程 计 信息 后 可 以 有 效 改 善 定 位 精度 ， 
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如 图 16 所 示 ，EKF BEA EA 
接近 由 RTK 提 供 的 真实 定位 


为 了 进一步 分 析 定 位 误差 ， 按 照 公 式 (27) 和 
公式 (28) 计算 误差 绝对 值 进 行 定量 分 析 ， 结 果 如 
图 17 和 表 2 所 示 。 
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b. 4E W BE 
图 17 实验 环境 下 农业 机 器 人 的 定位 误差 


Fig. 17 Localization error of agricultural robot in experimental environment 


由 图 17 及 表 2 中 数据 分 析 可 知 ， 在 实验 时 定位 
结果 的 误差 波动 多 出 现在 转向 处 ， 这 一 现象 与 仿真 
结果 相同 ， 主 要 是 由 激光 雷达 硬件 结构 以 及 3D- 
NDT 定 位 算法 原理 引起 。 虽 然 相 较 于 仿真 环境 ， 真 
实 场景 下 的 特征 会 更 多 ， 有 利于 提高 点 云 匹配 的 精 


度 ， 但 同时 ， 在 真实 场景 下 路 面 会 存在 一 定 颠 化 , 
也 会 导致 误差 绝对 值 曲线 出 现 较 大 震荡 。 在 本 研究 
所 采用 的 两 种 工 况 下 ， 若 仅 使 用 激光 雷达 进行 基 
于 3D-NDT 定 位 ,在 和 了 方向 上 的 平均 定位 误差 
分 别 为 7.0 和 5.5 cm; X 和 了 方向 上 的 最 大 误差 绝 
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表 2 实验 环境 下 农业 机 器 人 的 定位 误差 统计 表 


Table 2 Localization error statistical table of agricultural robot in experimental environment 


X 方 向 上 的 平均 误差 Y 方 向 上 的 平均 误差 X 方 向 上 的 最 大 误差 7 方 向 上 的 最 大 误差 


& fuk 
路 和 ism 绝对 值 /cm 绝对 值 /cm 绝对 值 /cm 绝对 值 /cm 
3D-NDT 定 位 算法 6.7 6.0 11.7 11.1 
转向 路 径 aoe aoa 
EKF 融 合 定位 算法 3.5 3.9 7.5 10.5 
3D-NDT 定 位 算法 7.3 5.0 16.8 13.2 
矩形 路 径 MP oiu 
EKF 融 合 定位 算法 3.0 2.6 8.4 8.7 
3D-NDT 定 位 算法 7.0 5:5 16.8 13.2 
综合 两 种 工 况 e 
EKF 融 合 定 位 算法 3.3 3.3 8.4 10.5 
算法 性 能 提升 /% 52.9 40.0 
对 值 分 别 为 16.8 和 13.2 cm， 已 经 不 能 很 好 地 满足 ”参考 文献 : 


农业 机 器 人 的 定位 需求 。 而 本 研究 提出 的 融合 定 
位 算法 在 X 和 了 方向 上 的 平均 定位 误差 分 别 为 3.3 
和 3.3 cm; XX 和 了 方向 上 的 最 大 误差 绝对 值 分 别 为 
8.4 和 10.5 cm。 相 较 于 传统 3D-NDT 定 位 算法 ,在 
X 方 向 上 定位 精度 提高 了 52.9%， 在 了 方向 上 提高 
T 40.0%， 并 且 降 低 了 运动 过 程 中 的 最 大 误差 绝对 
值 ， 能 够 有 效 提高 激光 雷达 在 农业 环境 中 定位 的 
精度 。 


针对 智慧 农业 果园 场景 中 GNSS 信号 较 弱 ， 可 
能 导致 定位 精度 下 降 这 一 问题 ， 提 出 一 种 基于 激光 
雷达 与 IMU 融合 的 定位 算法 ， 通 过 仿真 与 实验 对 算 
法 进行 了 验证 与 分 析 ， 主 要 结论 如 下 。 

1) 本 研究 使 用 扩展 卡尔 曼 滤 波 算法 ， 在 改进 
3D-NDT 定 位 算法 的 基础 上 ， 进 一 步 融合 IMU 里 程 
计 信 息 ， 实 现 农业 机 器 人 的 高 精度 定位 。 

2) 实验 结果 表明 ， 与 传统 3D-NDT 定 位 算法 
相 比 ， 提 出 的 融合 定位 方法 的 定位 精度 在 和 和 了 方 
向 上 分 别提 高 52.9% 和 40.0% ， 且 降低 了 运动 过 程 
中 的 最 大 误差 ， 减 小 误差 曲线 波动 的 同时 获得 了 更 
高 的 定位 精度 。 

3) 由 于 3D-NDT 定 位 算法 依赖 于 先 验 点 云 地 
图 的 准确 性 ， 且 当 点 云 角度 变化 较 大 时 ， 定 位 精度 
会 显著 下 降 。 

后 续 将 继续 优化 所 提出 的 融合 定位 算法 ， 减 少 
对 于 先 验 点 云 地 图 的 过 度 依赖 并 结合 其 他 定位 方 
法 ， 进 一 步 提 高 农业 机 器 人 的 定位 精度 。 


利益 冲突 声明 : 本 研究 不 存在 研究 者 以 及 与 公开 
研究 成 果 有 关 的 利益 冲突 。 
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Abstract: 
[Objective] High-precision localization technology serves as the crucial foundation in enabling the autonomous navigation operations 
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of intelligent agricultural robots. However, the traditional global navigation satellite system (GNSS) localization method faces numer- 
ous limitations, such as tree shadow, electromagnetic interference, and other factors in the agricultural environment brings challenges 
to the accuracy and reliability of localization technology. To address the deficiencies and achieve precise localization of agricultural ro- 
bots independent of GNSS, a localization method was proposed based on the fusion of three-dimensional light detection and ranging 
(LiDAR) data and inertial measurement unit (IMU) information to enhance localization accuracy and reliability. 

[Methods] LiDAR was used to obtain point cloud data in the agricultural environment and realize self-localization via point cloud 
matching. By integrating real-time motion parameter measurements from the IMU with LiDAR data, a high-precision localization so- 
lution for agricultural robots was achieved through a specific fusion algorithm. Firstly, the LiDAR-obtained point cloud data was pre- 
processed and the depth map was used to save the data. This approach could reduce the dimensionality of the original LiDAR point 
cloud, and eliminate the disorder of the original LiDAR point cloud arrangement, facilitating traversal and clustering through graph 
search. Given the presence of numerous distinct crops like trees in the agricultural environment, an angle-based clustering method was 
adopted. Specific angle-based clustering criteria were set to group the point cloud data, leading to the segmentation of different clus- 
ters of points, and obvious crops in the agricultural environment was effectively perceived. Furthermore, to improve the accuracy and 
stability of positioning, an improved three-dimensional normal distribution transform (3D-NDT) localization algorithm was proposed. 
This algorithm operated by matching the LiDAR-scanned point cloud data in real time with the pre-existing down sampled point cloud 
map to achieve real-time localization. Considering that direct down sampling of LiDAR point clouds in the agricultural environment 
could result in the loss of crucial environmental data, a point cloud clustering operation was used in place of down sampling operation, 
thereby improving matching accuracy and positioning precision. Secondly, to address potential constraints and shortcomings of using 
a single sensor for robot localization, a multi-sensor information fusion strategy was deployed to improve the localization accuracy. 
Specifically, the extended Kalman filter algorithm (EKF) was chosen to fuse the localization data from LiDAR point cloud and the 
IMU odometer information. The IMU provided essential motion parameters such as acceleration and angular velocity of the agricultur- 
al robot, and by combining with the LiDAR-derived localization information, the localization of the agricultural robot could be more 
accurately estimated. This fusion approach maximized the advantages of different sensors, compensated for their individual limita- 
tions, and improved the overall localization accuracy of the agricultural robot. 

[Results and Discussions] A series of experimental results in the Gazebo simulation environment of the robot operating system (ROS) 
and real operation scenarios showed that the fusion localization method proposed had significant advantages. In the simulation envi- 
ronment, the average localization errors of the proposed multi-sensor data fusion localization method were 1.7 and 1.8 cm, respective- 
ly, while in the experimental scenario, these errors were 3.3 and 3.3 cm, respectively, which were significantly better than the tradition- 
al 3D-NDT localization algorithm. These findings showed that the localization method proposed in this study could achieve high-pre- 
cision localization in the complex agricultural environment, and provide reliable localization assistance for the autonomous function- 
ing of agricultural robots. 

[Conclusions] The proposed localization method based on the fusion of LiDAR data and IMU information provided a novel localiza- 
tion solution for the autonomous operation of agricultural robots in areas with limited GNSS reception. Through the comprehensive 
utilization of multi-sensor information and adopting advanced data processing and fusion algorithms, the localization accuracy of agri- 
cultural robots could be significantly improved, which could provide a new reference for the intelligence and automation of agricultur- 
al production. 
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